﻿/*****************************************************************//**
 * \file   AuboRobotControl.h
 * \brief  奥博机械臂控制相关代码
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/
#pragma once
#include "Eigen/Dense"
#include <sys/socket.h>

#include "../RobotControlInterface.h""
 //兼容的写法需要的库
#include <ratio>
#include <chrono>
#include <thread>

#include <QFile>
#include <QTextStream>

#include <time.h>
//linux api
#include "../../../dependents/robotSDK/inc/AuboRobotMetaType.h"
#include "../../../dependents/robotSDK/inc/serviceinterface.h"
using namespace std::chrono;

struct MyStruct
{
	double** q_p;
	double** q_v;
	double** q_a;
};
/*
	机械臂通讯类
*/
class AuboRobotControl : public RobotControlInterface
{
public:
	AuboRobotControl();
	~AuboRobotControl();

public:

    ServiceInterface robotService;

	//AUBO机械臂 句柄
    //RSHD rshdHand = -1;
	bool bInitDone;
	bool bFollowStart;
    //linux
    aubo_robot_namespace::RobotDiagnosis ala;
    //RobotDiagnosis ala;
	double dTcp2Canbus4Points[4][6];
	double dLastJoint[6], dLastJointV[6], dLastJointA[6];
//	LARGE_INTEGER  liBegintime, liEndtime, lilasttime;
//	LARGE_INTEGER liQpfrequency;

    // 六十四位结构体，为防止控制过程中时间越界
    int64 liBegintime, liEndtime, lilasttime;
    int64 liQpfrequency;


    high_resolution_clock::time_point t1, t2;
	bool robotLogin(const char* addr, int port) override;
	bool robotShutdown() override;
	bool robotMoveJ(double* initPos) override;
	void robotMove(double* target_postion) override;
	void robotMove(Eigen::Matrix4d desired_EndMatrix) override;
	bool robotGetConnectState() override;
	void GetRealtimeState(SRobotRealTimeData& sRunTimedata) override;
	bool robotRemove() override;
	void test_hahaha()override;

	bool robotStartup();
	bool robotMoveFollow(double* initPos);
	bool robotMoveFollowOld(double* initPos);
	bool robotGetJoint(double* jointangle);
	void robotForward(double jointangle[6], Eigen::Matrix3d& robot_ori, Eigen::Vector3d& robot_trans);

    //bool robotInverse(double* joint_move, Pos* pos, Ori* ori, wayPoint_S* waypoint);
    bool robotInverse(double* joint_move, aubo_robot_namespace::Pos* pos,aubo_robot_namespace::Ori* ori,aubo_robot_namespace::wayPoint_S* waypoint);

	void robotInverse(Eigen::Matrix4d orientation_matrix, double* joint_target);
    bool robotGetJointStatus(aubo_robot_namespace::JointStatus *jointStatus);
	void GetRobotJointStatus(SRobotRealTimeData& sRunTimedata);
	//MyStruct Polynomia5_interpolation(double* Lastpoint, double* Newpoint, double* Lastpointv, double* Newpointv, double* Lastpointa, double* Newpointa, double time, double N);
	MyStruct* Polynomia5_interpolation(Eigen::VectorXd Lastpoint, Eigen::VectorXd Newpoint, Eigen::VectorXd Lastpointv, Eigen::VectorXd Newpointv, Eigen::VectorXd  Lastpointa, Eigen::VectorXd  Newpointa, double time, double N);

};
